#include <stdio.h>
#include <stdlib.h>
#include <hidapi/hidapi.h>
#include <inttypes.h> // 用于PRId16

// 新增：打印原始字节调试函数
void print_raw_bytes(const unsigned char *buf, int len) {
    printf("Raw byte data: ");
    for(int i=0; i<len; i++) {
        printf("%02X ", buf[i]);
    }
    printf("\n");
}

// 模拟机器人移动函数
void move_robot(float x, float y, float z, float rx, float ry, float rz) {

    const float clamp = 0.95; // 防止极值抖动
    x = (x > clamp) ? clamp : (x < -clamp) ? -clamp : x;
    y = (y > clamp) ? clamp : (y < -clamp) ? -clamp : y;
    z = (z > clamp)? clamp : (z < -clamp)? -clamp : z;
    rx = (rx > clamp)? clamp : (rx < -clamp)? -clamp : rx;
    ry = (ry > clamp)? clamp : (ry < -clamp)? -clamp : ry;
    rz = (rz > clamp)? clamp : (rz < -clamp)? -clamp : rz;

    printf("Moving robot: X: %.2f, Y: %.2f, Z: %.2f, RX: %.2f, RY: %.2f, RZ: %.2f\n", x, y, z, rx, ry, rz);
    // 这里可以添加实际控制机器人移动的代码
    // 例如，通过串口通信向机器人发送控制指令
    // 或者通过网络通信将控制信息发送给机器人的控制器
}

int main(int argc, char* argv[]) {
    hid_device *handle;
    int res;
    unsigned char buf[65];

    // 初始化 hidapi 库
    res = hid_init();
    if (res < 0) {
        printf("hid_init failed with error code %d\n", res);
        return 1;
    }

    // 打开 3D Connexion 设备
    handle = hid_open(0x256f, 0xc635, NULL);
    if (!handle) {
        printf("Unable to open device\n");
        hid_exit();
        return 1;
    }

    printf("Connected to 3D mouse. Press Ctrl+C to exit.\n");

    // 读取设备输入
    while (1) {
        res = hid_read(handle, buf, 65);
        if (res < 0) {
            printf("Unable to read data\n");
            break;
        }

        if (res > 0) {
            print_raw_bytes(buf, 20); // 打印前16字节用于调试

            // 解析数据，16位有符号整数解析（大端序）
            int16_t raw_x  = (int16_t)((buf[1] << 8) | buf[2]);   // X轴：0x0200 = 512
            int16_t raw_y  = (int16_t)((buf[3] << 8) | buf[4]);   // Y轴：0x1700 = 5888（注意此值异常，见下文）
            int16_t raw_z  = (int16_t)((buf[5] << 8) | buf[6]);   // Z轴：0x0000 = 0
            int16_t raw_rx = (int16_t)((buf[7] << 8) | buf[8]);   // RX轴：0x00A9 = 169
            int16_t raw_ry = (int16_t)((buf[9] << 8) | buf[10]);  // RY轴：0x9C60 = -25408（修正符号！）
            int16_t raw_rz = (int16_t)((buf[11] << 8) | buf[12]); // RZ轴：0x3BFC = 15356

            // 归一化到[-1, 1]范围（3D Connexion典型输出范围±32768）
            float x = (float)raw_x / 32768.0f;
            float y = (float)raw_y / 32768.0f;
            float z = (float)raw_z / 32768.0f;
            float rx = (float)raw_rx / 32768.0f;
            float ry = (float)raw_ry / 32768.0f;
            float rz = (float)raw_rz / 32768.0f;

            // 输出原始值和归一化值（用于调试）
            // printf("Raw: X:%6" PRId16 " Y:%6" PRId16 " Z:%6" PRId16 
            //     " RX:%6" PRId16 " RY:%6" PRId16 " RZ:%6" PRId16 "\n",
            //     raw_x, raw_y, raw_z, raw_rx, raw_ry, raw_rz);

            // 调用模拟机器人移动函数
            // move_robot(x, y, z, rx, ry, rz);


        }
    }

    // 关闭设备并退出 hidapi 库
    hid_close(handle);
    hid_exit();

    return 0;
}    